#include <xc.h>
#include <can.h>
#include <stdlib.h>

#include "Serial.h"

#include "MyCan.h"
#include "TemperatureStuff.h"
#include "CanMessage.h"
#include "Config.h"

void SendCANMessage(uint32_t identifier, unsigned char* data, char bufferNumber)
{
        CAN1SendMessage(CAN_TX_SID(SID_FROM_FULL_ID(identifier)) &
                    (CAN_TX_EID_EN) & (CAN_SUB_NOR_TX_REQ)
            , CAN_TX_EID(EID_FROM_FULL_ID(identifier)) &
                    (CAN_NOR_TX_REQ)
            , data, 8, bufferNumber);
}

unsigned int CAN1GetSID0()
{
    return C1RX0SIDbits.SID;
}

uint32_t CAN1GetEID0()
{
   return (((uint32_t)C1RX0EID) << 6) | C1RX0DLCbits.EID5_0;
}

unsigned int CAN1GetSID1()
{
    return C1RX1SIDbits.SID;
}

uint32_t CAN1GetEID1()
{
    return (((uint32_t)C1RX1EID) << 6) | C1RX1DLCbits.EID5_0;
}

//MightNotWork
uint32_t CAN1GetRecieveExtendedID0()
{
    return ((uint32_t)CAN1GetSID0() << 18) | CAN1GetEID0();
}

uint32_t CAN1GetRecieveExtendedID1()
{
    return ((uint32_t)CAN1GetSID1() << 18) | CAN1GetEID1();
}

char CAN1GetDLC1()
{
    return C1RX1DLCbits.DLC;
}

char CAN1GetDLC0()
{
    return C1RX0DLCbits.DLC;
}

unsigned long StitchDAInEID(unsigned long EID, unsigned char DA)
{
    return EID | ((unsigned long)DA << 8);
}

uint32_t StitchDAIntoFullIdentifier(uint32_t id, unsigned char DA)
{
    return id | ((uint32_t)DA << 8);
}

uint32_t StitchSAIntoFullIdentifier(uint32_t id, unsigned char SA)
{
    return id | SA;
}

unsigned char CAN1GetSA()
{
    return ((C1RX0EID << 6) | C1RX0DLCbits.EID5_0) & 0xff;
}

unsigned char ExtractSAFromEID(int EID)
{
    return EID & 0xff;
}

void HandleCAN1Buffer0()
{
    if(CAN1IsRXReady(0))
    {
        HandleCANRX0();
        CAN1SetRXMode(0, CAN_RXFUL_CLEAR & CAN_BUF0_DBLBUFFER_EN);
    }
}

void HandleCAN1Buffer1()
{
    if(CAN1IsRXReady(1))
    {
        HandleCANRX1();
        CAN1SetRXMode(1, CAN_RXFUL_CLEAR);
    }
}

void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void)
{
    if (C1INTFbits.TX0IF) C1INTFbits.TX0IF = 0;
    if (C1INTFbits.TX1IF) C1INTFbits.TX1IF = 0;
    if (C1INTFbits.TX2IF) C1INTFbits.TX2IF = 0;
    if (C1INTFbits.RX0IF)
    {
        C1INTFbits.RX0IF = 0; // Add code to read buffer 0
        HandleCAN1Buffer0();
        C1RX0CONbits.RXFUL = 0;
    }
    if (C1INTFbits.RX1IF) 
    {
        C1INTFbits.RX1IF = 0;
        HandleCAN1Buffer1();    // Add code to read buffer 1
        C1RX1CONbits.RXFUL = 0;
    }
    if (C1INTFbits.WAKIF) C1INTFbits.WAKIF = 0; // Add wake-up handler code
    if (C1INTFbits.ERRIF) C1INTFbits.ERRIF = 0; // Add error handler code
    if (C1INTFbits.IVRIF) C1INTFbits.IVRIF = 0;
    if ( (C1INTF & C1INTE) == 0 ) IFS1bits.C1IF = 0;
}

void InitializeCAN()
{
    char FilterNo,tx_rx_no;
    /* Holds the data received */
    /* Set request for configuration mode */
    CAN1SetOperationMode(CAN_IDLE_CON & CAN_MASTERCLOCK_1 &
            CAN_REQ_OPERMODE_CONFIG & CAN_CAPTURE_DIS);
    while(C1CTRLbits.OPMODE <=3);
    /* Load configuration register */
    //CAN1Initialize(CAN_SYNC_JUMP_WIDTH1 & CAN_BAUD_PRE_SCALE(1),
    //        CAN_WAKEUP_BY_FILTER_DIS & CAN_PHASE_SEG2_TQ(6) &
    //        CAN_PHASE_SEG1_TQ(8) & CAN_PROPAGATIONTIME_SEG_TQ(1) &
    //        CAN_SEG2_FREE_PROG & CAN_SAMPLE1TIME);

//    CAN1Initialize(0x0000, 0x05B8); //as calculated by the CAN timing tool
#ifdef USE_CRYSTAL
    CAN1Initialize(0x0000, 0x0290);
#else
    CAN1Initialize(0x0001, 0x0290);
#endif
    /* Load Acceptance filter register */
    FilterNo = 0;
    CAN1SetFilter(FilterNo, CAN_FILTER_SID(SID_FROM_FULL_ID(CHARGER_STATUS_MESSAGE_BASE)) & CAN_RX_EID_EN, CAN_FILTER_EID(EID_FROM_FULL_ID(CHARGER_STATUS_MESSAGE_BASE)));
    /* Load mask filter register */
    CAN1SetMask(FilterNo, CAN_MASK_SID(SID_FROM_FULL_ID(MASK_MATCH_IGNORE_SA)) & CAN_MATCH_FILTER_TYPE, CAN_MASK_EID(MASK_MATCH_IGNORE_SA));

    CAN1SetFilter(2, CAN_FILTER_SID(SID_FROM_FULL_ID(CHARGE_COMMAND_MESSAGE_BASE)) & CAN_RX_EID_EN, CAN_FILTER_EID(EID_FROM_FULL_ID(CHARGE_COMMAND_MESSAGE_BASE)));

    CAN1SetMask(1,CAN_MASK_SID(SID_FROM_FULL_ID(MASK_MATCH_ANYTHING)) & CAN_MATCH_FILTER_TYPE, CAN_MASK_EID(MASK_MATCH_ANYTHING));
    /* Set transmitter and receiver mode */
    tx_rx_no = 0;
    CAN1SetTXMode(0, CAN_TX_STOP_REQ & CAN_TX_PRIORITY_HIGH );
    CAN1SetTXMode(1, CAN_TX_STOP_REQ & CAN_TX_PRIORITY_HIGH_INTER);
    CAN1SetTXMode(2, CAN_TX_STOP_REQ & CAN_TX_PRIORITY_HIGH_INTER);
    CAN1SetRXMode(0, CAN_RXFUL_CLEAR & CAN_BUF0_DBLBUFFER_DIS);
    CAN1SetRXMode(1, CAN_RXFUL_CLEAR);
     /* Set request for Normal mode */
    CAN1SetOperationMode(CAN_IDLE_CON & CAN_CAPTURE_DIS & CAN_MASTERCLOCK_1 & CAN_REQ_OPERMODE_NOR);
    while(C1CTRLbits.OPMODE !=0);

    ConfigIntCAN1(CAN_INDI_ERR_DIS & CAN_INDI_INVMESS_DIS &
                  CAN_INDI_RXB0_EN & CAN_INDI_RXB1_EN &
                  CAN_INDI_TXB0_DIS & CAN_INDI_TXB1_DIS &
                  CAN_INDI_TXB2_DIS & CAN_INDI_WAK_DIS
                 , CAN_INT_ENABLE & CAN_INT_PRI_3);
    EnableIntCAN1;

}

int DoesSidEidMatchMessageBase(int SID, int EID, uint32_t messageBase)
{
    return SID == SID_FROM_FULL_ID(messageBase) && (EID & 0xff00) == EID_FROM_FULL_ID(messageBase);
}

int DoesIDMatchMessageBase_IgnoreSA(uint32_t ID, uint32_t messageBase)
{
    return (ID & MASK_MATCH_IGNORE_SA) == messageBase;
}

int DoesIDMatchMessageBase_IgnoreSADA(uint32_t ID, uint32_t messageBase)
{
    return (ID & MASK_MATCH_IGNORE_SA_AND_DA) == (messageBase & MASK_MATCH_IGNORE_SA_AND_DA);
}

void MC_PrintCANStatus()
{
    __builtin_disi(0x3FFF);
    char rxBuffer0Full = CAN1IsRXReady(0);
    char rxBuffer1Full = CAN1IsRXReady(1);
    char txBuffer0Empty = CAN1IsTXReady(0);
    char txBuffer1Empty = CAN1IsTXReady(1);
    char CANOff = CAN1IsBusOff();
    __builtin_disi(0);

    PutString("CAN Status");
    PutString("\rRX Buffer 0 Full: ");
    char buffer[10];
    itoa(buffer, rxBuffer0Full, 10);
    PutString(buffer);
    PutString("\rRX Buffer 1 Full: ");
    itoa(buffer, rxBuffer1Full, 10);
    PutString(buffer);
    PutString("\rTX Buffer 0 Empty: ");
    itoa(buffer, txBuffer0Empty, 10);
    PutString(buffer);
    PutString("\rTX Buffer 1 Empty: ");
    itoa(buffer, txBuffer1Empty, 10);
    PutString(buffer);
    PutString("\rCAN Off: ");
    itoa(buffer, CANOff, 10);
    PutString(buffer);
    Putchar('\r');
}